/*
 * WebotsSupervisor.h
 *
 *  Created on: 29 Jun 2012
 *      Author: bweel
 */

#ifndef WEBOTSSUPERVISOR_H_
#define WEBOTSSUPERVISOR_H_

#include <webots/Robot.hpp>
#include <vector>
#include <sstream>

#include "WorldModel.h"
#include "DemoSupervisor.h"
#include "Controller.h"

using namespace webots;

class WebotsSupervisor : public DemoSupervisor {
public:
	WebotsSupervisor(int argc, char **argv, RobotType *robot);
	virtual ~WebotsSupervisor();
	virtual void controlStep();
	virtual bool initialise(int argc, char **argv);

private:
	// load and enable sensors and other hardware
	void initialiseRobotHardware();
	void setOrganismChannel();

	void loadWorldModel();
	void applyWorldModel();

	void receiveMessages();
	void sendMessages();

    void calculateOrganismSize();
    
	bool isConnected(size_t id);
	void connect(size_t id);
	void disconnect(size_t id);
	bool detectPresence(size_t id);

	int numConnected();
	int numNotReceived(int counted[], int num);

    void setLEDColour(std::pair<int,int> ledColour);
    void setLEDs(std::vector<LED*> toset, int colour);
    
    bool checkSupport();
    bool parseParameters(int argc, char **argv);

	std::string getOpenConnection();
	int sum(int counted[],int num);

    int controlType;
    Controller *control;
    
    // Do we want to load a NN from a file
    bool loadNN;
    std::string NNFile;
    
	WorldModel worldModel;
	double startTime;
    
	// Count of organism size subtrees
	int counted[4];

	Servo *antiServo;
    
    RobotType *robot;
};

#endif /* WEBOTSSUPERVISOR_H_ */
